package source;

import source.Enums.Direction;
import source.Enums.Side;
import source.calcCmd.RealLifeProperties;
import source.calcCmd.RobotCommand;
import source.calcCmd.State;
import source.calcCmd.Surf;
import source.control.CmdInterface;
import source.control.Command;
import source.control.Serial;
import source.realRobot.RealRobot;

public class RobotControl {
	
	private Serial port;
	private CmdInterface cmdInterface;
	
	private ControlMode mode;
	private Connection connection;
	
	private State afterCmds;
	
	private RealRobot robot;
	private Surf surf;
	
	//private int prevVR;
	//private int prevVL;
	
	public void connect(int portNumber) throws Exception{
		
		try {
			portInit(portNumber);
			
			connection = Connection.connected;
			
			cmdInterface = new CmdInterface(port, this);
			
			afterCmds = new State();
			
			robot = new RealRobot();
			surf = new Surf();
	
			mode = ControlMode.withoutModification;
		} catch (Exception e){

			connection = Connection.disconnected;
			throw e;
			
		}
	
	}
	
	public StateAndCommand addCommand(double vL, double vR, int t){
		
		Command cmd = new Command(t,vL,vR);
		
		return addCommand(cmd);
		
	}
	
	public StateAndCommand addCommand(Command cmd){
		
		RobotCommand usedCmd = null;
		
		RobotCommand idealCmd = 
				new RobotCommand(	cmd,
									cmdInterface.getPrevSp(Side.LEFT),
									cmdInterface.getPrevSp(Side.RIGHT));
		if (cmd.getV(Side.LEFT) == 0 && cmd.getV(Side.RIGHT) == 0){
			Direction dirL = (cmdInterface.getPrevV(Side.LEFT) < 0 ? Direction.BACKWARD : Direction.FORWARD);
			Direction dirR = (cmdInterface.getPrevV(Side.RIGHT) < 0 ? Direction.BACKWARD : Direction.FORWARD);
			idealCmd = RobotCommand.toZero(cmdInterface.getPrevSp(Side.LEFT), cmdInterface.getPrevSp(Side.RIGHT), dirL, dirR);
		}
		
		
		
		int signVL = (int)Math.signum(cmd.getV(Side.LEFT));
		int signVR = (int)Math.signum(cmd.getV(Side.RIGHT));
		
		if ( signVL*Math.signum(cmdInterface.getPrevV(Side.LEFT)) < 0 || signVR*Math.signum(cmdInterface.getPrevV(Side.RIGHT)) < 0){
			
			RobotCommand toZero = new RobotCommand(	0,0,1000,
													cmdInterface.getPrevSp(Side.LEFT),
													cmdInterface.getPrevSp(Side.RIGHT));
			
			if ( cmdInterface.getPrevV(Side.LEFT) < 0 ){
				toZero.changeDir(Side.LEFT, Direction.BACKWARD);
			}
			if ( cmdInterface.getPrevV(Side.RIGHT) < 0 ){
				toZero.changeDir(Side.RIGHT, Direction.BACKWARD);
			}
			
			cmdInterface.addCommand(toZero);
			
			afterCmds = robot.runCmd(afterCmds,toZero);
			
			
			
			cmdInterface.setPrevSp(Side.LEFT, RealLifeProperties.timerMaxValue);
			cmdInterface.setPrevSp(Side.RIGHT, RealLifeProperties.timerMaxValue);
			
			cmdInterface.setPrevV(Side.LEFT,0);
			cmdInterface.setPrevV(Side.RIGHT,0);
		}
		
		
		ControlMode backup = mode;
		if (cmd.getV(Side.LEFT) == 0 && cmd.getV(Side.RIGHT) == 0){
			mode = ControlMode.withoutModification;
		}
		
		switch (mode){
		
		case withoutModification:

			usedCmd = idealCmd;
			afterCmds = robot.runCmd(	afterCmds,
										idealCmd );
		
			//usedCmd.setV(usedCmd.getV(Side.LEFT)* (idealCmd.getDirL() == Direction.FORWARD ? 1 : -1), Side.LEFT);
			break;
		
		case changeToIdeal:
			
			RobotCommand surfCmd = surf.evaluate(afterCmds, idealCmd);
			
			if (cmd.getV(Side.LEFT) == 0){
				surfCmd.newCommand(0, surfCmd.getRealV(Side.RIGHT));
			} else if (cmd.getV(Side.RIGHT) == 0){
				surfCmd.newCommand(surfCmd.getRealV(Side.LEFT), 0);
			} else if (cmd.getV(Side.RIGHT) == 0 && cmd.getV(Side.LEFT) == 0){
				surfCmd.newCommand(0, 0);
			}
			
			usedCmd = new RobotCommand(surfCmd);
			
			afterCmds = robot.runCmd(afterCmds, surfCmd);
			
			break;
		}
		
		if (cmd.getV(Side.LEFT) == 0 && cmd.getV(Side.RIGHT) == 0){
			mode = backup;
		}
		cmdInterface.addCommand(usedCmd);
		
		cmdInterface.setPrevV(Side.LEFT,signVL);
		cmdInterface.setPrevV(Side.RIGHT,signVR);
		
		
		
		int time = cmd.getTime();
		double vL = RealLifeProperties.speedToV(usedCmd.getSpeedL());
		if (usedCmd.getDirL() == Direction.BACKWARD){
			vL *= -1;
		}
		double vR = RealLifeProperties.speedToV(usedCmd.getSpeedR());
		if (usedCmd.getDirR() == Direction.BACKWARD){
			vR *= -1;
		}
		Command finalCmd = new Command(time,vL,vR);
		
		return new StateAndCommand(afterCmds,finalCmd);
	}
	
	public void startRunning(){
		cmdInterface.startRunning();
	}
	
	public void stopRunning(){
		cmdInterface.stopRunning();
	}
	
	public void disconnect(){
		port.closeport();
		
		connection = Connection.disconnected;
	}
	
	public void setControlMode(ControlMode mode){
		this.mode = mode;
	}
	
	public Connection getConnection(){
		return connection;
	}
	
	public ControlMode getControlMode(){
		return mode;
	}
	
	public ExecutionState getExecutionState(){
		return cmdInterface.getExecState();
	}
	
	private void portInit(int n){
	
		port = new Serial("COM" + Integer.toString(n));
		
		
		port.write("reset");
		try {
			Thread.sleep(100);
		} catch (InterruptedException e2) {
			e2.printStackTrace();
		}
		
		
		port.write("ping");
		try {
			Thread.sleep(50);
		} catch (InterruptedException e1) {
			e1.printStackTrace();
		}
		port.waitForReady();
		System.out.println("USB interface inited");
	}
	
	public enum ControlMode{
		changeToIdeal, withoutModification;
	}
	
	public enum Connection{
		connected, disconnected;
	}
	
	public enum ExecutionState{
		running, ready;
	}
}
